#requires the robot team module
require 'control_lib/level2/main/robot_team.rb'

#requires the robot teammap module
require 'control_lib/level2/support/teammap.rb'



team1= RobotTeam.new("searchteam","10.1.157.59",3000)
team1.addrobot("USARBot.P2AT","-6.0,-2.0,1.8","robot1")



sleep(1)

map=Teammap.new(team1)
map.autoupdatemap(true)
map.autologmap(true)


team1.robotno.each{|robot|
  robot.colguardmode
}


cbot=team1.robotno[0]

cbot.smartfor(1.0,2.0)
cbot.smartright(6.28/4)

#Main Loop
oldtime=Time.now
totaltime=0.0
while(totaltime<=60.0*12.0)
  totaltime=(Time.now-oldtime)+totaltime
  oldtime=Time.now
  puts "Time Taken:#{totaltime}"
  leftdist=cbot.fSONARSEN[0].to_f
  frontdist=(cbot.fSONARSEN[4].to_f+cbot.fSONARSEN[3].to_f)/2.0
  reardist=cbot.fSONARSEN[4].to_f
  rightdist=cbot.fSONARSEN[7].to_f
    
  if(frontdist<0.6 )
     puts frontdist
      if(leftdist>1.0) #left is free
        cbot.smartleft(6.28/4)
      elsif(rightdist>1.0) #right is free
        cbot.smartright(6.28/4)
      else
        cbot.smartleft(6.28/2)
      end        
  else
   if(leftdist<0.5) # wall on left side
    #veer out
    puts "Veering out "
    while (leftdist<=0.5 or reardist<0.5)
      move=Thread.new{
      leftdist=cbot.fSONARSEN[0].to_f
      reardist=cbot.fSONARSEN[4].to_f
      cbot.smartright(0.6)
      cbot.smartfor(0.4,2.0)
      puts "Veering out  #{leftdist}"}
      move.join
    end
  elsif (leftdist>1.2 ) #wall on left side 
    #veer in
    puts "Veering in"
     while (leftdist>=1.2  or frontdist<0.5)
      move=Thread.new{
      cbot.smartleft(0.6)
      cbot.smartfor(0.4,2.0)
      leftdist=cbot.fSONARSEN[0].to_f
      (cbot.fSONARSEN[4].to_f+cbot.fSONARSEN[3].to_f)/2.0
      
      puts "Veering in  #{leftdist}"}
      move.join
    end
  else
    cbot.smartfor(0.5,2.0)
  end  
  end
    
    
end  #while
 

 
 


sleep(20)